#include "main.h"
#include "mecanum.h"
#include "bsp_rc.h"
#include "remote_control.h"
#include "math.h"

#define Sqrt sqrt(2.0)/2

Chassis_Speed chassis;
Wheel_Speed wheel;
float wheel_rpm[4];//代表四个轮子速度


/*以下的vx vy都以rpm为单位，*/
const Chassis_Speed *get_chassis_targetspeed(void)
{
    return &chassis;
}

const Wheel_Speed *get_wheel_speed(void)
{
    return &wheel;
}

void Data_Prepare(Chassis_Speed *chassis)
{
    chassis->vx = 1.5*get_remote_control_point()->rc.ch[3];//左摇杆上下，范围-660~660
    chassis->vy = 1.5*get_remote_control_point()->rc.ch[2];//左摇杆左右，范围-660~660
    chassis->vw = 1.5*get_remote_control_point()->rc.ch[0];//右摇杆左右,定义-660为逆时针转，正660为顺时针转

}

void Motor_Cal(Chassis_Speed *chassis,Wheel_Speed *wheel,uint8_t Wheel_MODE)//这里相当于得到了目标速度
{
    Data_Prepare(chassis);

 
    if(Wheel_MODE == Wheel_MAC) //麦轮
    {
        wheel->wheel_0 = chassis->vx + chassis->vy + chassis->vw;//左前轮，第一象限
        wheel->wheel_1 = -chassis->vx - chassis->vy + chassis->vw;//左后轮，第二象限
        wheel->wheel_2 = -chassis->vx + chassis->vy + chassis->vw;//右后轮，第三象限
        wheel->wheel_3 = chassis->vx - chassis->vy + chassis->vw;//右前轮，第四象限
    }
    
    else if(Wheel_MODE == Wheel_Omni)//全向轮
    {          
        wheel->wheel_0 = (-Sqrt)*chassis->vx + (Sqrt)*chassis->vy + chassis->vw;//左前轮，第一象限
        wheel->wheel_1 = (-Sqrt)*chassis->vx + (-Sqrt)*chassis->vy + chassis->vw;//左后轮，第二象限
        wheel->wheel_2 = (Sqrt)*chassis->vx + (-Sqrt)*chassis->vy + chassis->vw;//右后轮，第三象限
        wheel->wheel_3 = (Sqrt)*chassis->vx + (Sqrt)*chassis->vy + chassis->vw;//右前轮，第四象限
    }


  
}



